using System;
using System.Collections.Generic;
namespace XNAGameEngine.DataStructures
{
    /// <summary>
    /// Priority Queue implementation
    /// </summary>
    public class PriorityQueue<T>
        where T : IComparable<T>
    {
        #region local fields

        /// <summary>
        /// storage container for all of the objects in the queue
        /// </summary>
        private readonly List<T> _heapContents;

        #endregion

        #region properties

        /// <summary>
        /// Gets the number of objects in the queue
        /// </summary>
        public virtual int Count
        {
            get { return _heapContents.Count - 1; }
        }

        #endregion

        #region constructors

        /// <summary>
        /// Construct a new priority queue
        /// </summary>
        public PriorityQueue()
        {
            _heapContents = new List<T> { default(T) };
        }

        #endregion

        #region local methods

        /// <summary>
        /// Get the head element of the queue
        /// </summary>
        /// <returns>the head element in the queue</returns>
        public T Peek()
        {
            if (Count == 0)
                return default(T);
            return _heapContents[1];
        }

        /// <summary>
        /// Put a new item in the queue
        /// </summary>
        public void Enqueue(T value)
        {
            _heapContents.Add(value);
            PercolateUp(_heapContents.Count - 1);
        }

        /// <summary>
        /// Returns and remove the head element, maintain heap structure
        /// </summary>
        /// <returns>returns the head element in the queue</returns>
        public T Dequeue()
        {
            if (Count == 0)
                return default(T);

            T temp = _heapContents[1];

            if (_heapContents.Count > 2)
            {
                T lastValue = _heapContents[_heapContents.Count - 1];

                _heapContents.RemoveAt(_heapContents.Count - 1);
                _heapContents[1] = lastValue;
                PercolateDown(1);
            }
            else _heapContents.RemoveAt(1);

            return temp;
        }

        /// <summary>
        /// Restore the heap after an insertion
        /// </summary>
        /// <param name="index">the index to start</param>
        private void PercolateUp(int index)
        {
            while (IsParentLarger(index))
            {
                T parentValue = _heapContents[index >> 1];
                T childValue = _heapContents[index];

                _heapContents[index >> 1] = childValue;
                _heapContents[index] = parentValue;

                index = index >> 1;
            }
        }

        /// <summary>
        /// Restore the heap after a removal
        /// </summary>
        /// <param name="index">the index to start</param>
        private void PercolateDown(int index)
        {
            while (IsLeftChildSmaller(index) || IsRightChildSmaller(index))
            {
                int compareResult = CompareChildren(index);

                if (compareResult == -1)
                {
                    T parentValue = _heapContents[index];
                    T leftChildValue = _heapContents[index << 1];

                    _heapContents[index] = leftChildValue;
                    _heapContents[index << 1] = parentValue;

                    index = index << 1;
                }
                else if (compareResult == 1)
                {
                    T parentValue = _heapContents[index];
                    T rightChildValue = _heapContents[(index << 1) + 1];

                    _heapContents[index] = rightChildValue;
                    _heapContents[(index << 1) + 1] = parentValue;

                    index = (index << 1) + 1;
                }
            }
        }

        /// <summary>
        /// Returns if parent is larger than child
        /// </summary>
        /// <param name="index">the index to check</param>
        /// <returns>true if parent is larger than child, false otherwise</returns>
        private bool IsParentLarger(int index)
        {
            if (index == 1)
                return false;
            return _heapContents[index >> 1].CompareTo(_heapContents[index]) > 0;
        }

        /// <summary>
        /// Returns if left child is smaller than parent or false if a left 
        /// child does not exist.
        /// </summary>
        /// <param name="index">the index to check</param>
        /// <returns>true if left child is smaller than parent, false otherwise</returns>
        private bool IsLeftChildSmaller(int index)
        {
            if (index << 1 >= _heapContents.Count)
                return false;

            return _heapContents[index << 1].CompareTo(_heapContents[index]) < 0;
        }

        /// <summary>
        /// Returns if the right child is smaller than the parent or false if
        /// right child does not exist.
        /// </summary>
        /// <param name="index">the index to check</param>
        /// <returns>true if right child is smaller than parent, false otherwise</returns>
        private bool IsRightChildSmaller(int index)
        {
            if (index << 1 + 1 >= _heapContents.Count)
                return false;

            return _heapContents[(index << 1) + 1].CompareTo(_heapContents[index]) < 0;
        }

        /// <summary>
        /// Compares children, returns -1 if left child is smaller, 1 if 
        /// right child is smaller, or 0 if neither is smaller than parent
        /// </summary>
        /// <param name="index"></param>
        /// <returns>-1 for left child smaller, 1 for right child smaller,
        /// 0 for neither is smaller than parent</returns>
        private int CompareChildren(int index)
        {
            bool leftChildSmaller = IsLeftChildSmaller(index);
            bool rightChildSmaller = IsRightChildSmaller(index);

            if (!leftChildSmaller && !rightChildSmaller)
                return 0;

            if (leftChildSmaller && rightChildSmaller)
            {
                if (_heapContents[index << 1].CompareTo(_heapContents[(index << 1) + 1]) <= 0)
                    return -1;

                return 1;
            }

            if (leftChildSmaller)
                return -1;

            return 1;
        }

        #endregion
    }
}